#include "XLpa.h"
#include <cmath>
#include <fstream>
#include <cassert>
#include <cstdio>


SNR::SNR() {
	s = 0.;
	n = 0;
}


void SNR::add(double x, double y) {
	x = x - y;
	s += x;
	++n;
}


void SNR::merge(const SNR &snr) {
	s += snr.s;
	n += snr.n;
}


double SNR::value() {
	if (n == 0)
		return 999;
	else
		return s / n;
}


XSegment::XSegment(int channels, int n, int hlen)
		: channels(channels), n(n), hlen(hlen) {
	assert(hlen >= 1 && hlen <= n);
	a = new XSReal*[channels];
	c = new XSReal*[channels];
	for (int i = 0; i < channels; ++i) {
		a[i] = new XSReal[hlen];
		c[i] = new XSReal[hlen];
	}
	theta = new double*[hlen];
	t0 = new double[hlen];
	for (int i = 0; i < hlen; ++i) {
		theta[i] = new double[hlen];
	}
	t = new double[n];
	r = new double[n];
	__buf = new short[hlen];
}


XSegment::~XSegment() {
	for (int i = 0; i < channels; ++i) {
		delete a[i];
		delete c[i];
	}
	delete a;
	delete c;
	for (int i = 0; i < hlen; ++i) {
		delete theta[i];
	}
	delete t0;
	delete theta;
	delete t;
	delete r;
	delete __buf;
}


void XSegment::print() {
	printf("XSegment::print()\n");
	for (int i = 0; i < channels; ++i)
		for (int j = 0; j < hlen; ++j) {
			printf("a[%d][%d]=", i, j);
			printf("%7.4lf", a[i][j]);
			printf("\tc[%d][%d]=%7.4lf\n", i, j, c[i][j]);
		}
}


void XSegment::load(std::ifstream &file) {
	for (int i = 0; i < channels; ++i) {
		file.read((char*)__buf, sizeof(short) * hlen);
		for (int j = 0; j < hlen; ++j)
			a[i][j] = __buf[j];
	}
	for (int i = 0; i < channels; ++i) {
		file.read((char*)__buf, sizeof(short) * hlen);
		for (int j = 0; j < hlen; ++j)
			c[i][j] = __buf[j] / 10000.;
	}
	file.read((char*)&period, sizeof(int));
	file.read((char*)&bestInd, sizeof(int));
	float tmp;
	file.read((char*)&tmp, sizeof(float));
	bestHeight = tmp;
}


void XSegment::save(std::ofstream &file) {
	for (int i = 0; i < channels; ++i) {
		for (int j = 0; j < hlen; ++j)
			__buf[j] = a[i][j];
		file.write((char*)__buf, sizeof(short) * hlen);
	}
	for (int i = 0; i < channels; ++i) {
		for (int j = 0; j < hlen; ++j) {
			double x = c[i][j];
			if (x < -3) x = -3;
			if (x > +3) x = +3;
			__buf[j] = short(x * 10000.);
		}
		file.write((char*)__buf, sizeof(short) * hlen);
	}
	file.write((char*)&period, sizeof(int));
	file.write((char*)&bestInd, sizeof(int));
	float tmp = (float)bestHeight;
	file.write((char*)&tmp, sizeof(float));
}


void XSegment::findPeriod() {
	int ind = hlen;
	for (int i = hlen; i < n; ++i)
		if ((t[i]) > (t[ind]))
			ind = i;
	double peak = t[ind];
	std::vector<int> towers;
	for (int le = hlen, ri; le < n; le = ri) {
		while (le < n && t[le] < peak * 0.70)
			++le;
		if (le >= n)
			break;
		ri = le;
		while (ri < n && t[ri] >= peak * 0.40)
			++ri;
		int k = le;
		for (int i = le; i < ri; ++i)
			if (t[i] > t[k])
				k = i;
		towers.push_back(k);
	}
	period = n;
	if (towers.size() > 1) {
		int maxD = 0;
		for (size_t i = 1; i < towers.size(); ++i)
			maxD = std::max(maxD, towers[i] - towers[i-1]);
		double sumD = 0;
		int cntD = 0;
		for (size_t i = 1; i < towers.size(); ++i) {
			double d = towers[i] - towers[i-1];
			if (d >= maxD * 0.8) {
				sumD += d;
				cntD++;
			}
		}
		if (cntD == 0) {
			cntD = 1;
			sumD = maxD;
		}
		period = sumD / cntD;
	}
}


// Minimize \sum_i (t_i - c r_i)^2
double XSegment::bestScale() {
	double a = 0;
	double b = 0;
	for (int i = 0; i < n; ++i) {
		a += r[i] * r[i];
		b += - 2 * r[i] * t[i];
	}
	return - b / 2 / a;
}


void XSegment::updateStart(int p, int ind) {
	for (int i = 0; i < n; ++i)
		r[i] = 0.;
	for (int i = ind; i < n; i += period)
		r[i] = 1.;

	for (int i = hlen; i < n; ++i)
		for (int j = 0; j < hlen; ++j)
			r[i] += c[p][j] * r[i - j - 1];
	double h = bestScale();
	double delta = 0;
	for (int i = 0; i < n; ++i) {
		r[i] *= h;
		delta += sqr(r[i] - t[i]);
	}
	if (delta < bestDelta) {
		bestDelta = delta;
		bestInd = ind;
		bestHeight = h;
	}
}


void XSegment::make(double samples[]) {
	for (int p = 0; p < channels; ++p) {
		#define s(i) ((double)samples[(i) * channels + (p)])
		for (int i = 0; i < hlen; ++i)
			a[p][i] = s(i);
		for (int i = 0; i < hlen; ++i)
			for (int j = 0; j < hlen; ++j) {
				theta[i][j] = 0.;
				for (int k = hlen; k < n; ++k)
					theta[i][j] += s(k - i - 1) * s(k - j - 1);
			}
		for (int i = 0; i < hlen; ++i) {
			t0[i] = 0.;
			for (int k = hlen; k < n; ++k)
				t0[i] += s(k - i - 1) * s(k);
		}
		solveEquation(theta, t0, c[p], hlen);

		for (int i = 0; i < hlen; ++i)
			t[i] = s(i);
		for (int i = hlen; i < n; ++i) {
			t[i] = 0;
			for (int j = 0; j < hlen; ++j)
				t[i] += c[p][j] * t[i - j - 1];
		}
		for (int i = 0; i < n; ++i)
			t[i] = s(i) - t[i];
		findPeriod();

		bestDelta = 1E50;
		for (int i = 0; i < period; ++i)
			updateStart(p, i);
		updateStart(p, bestInd);

		//printf("p=%d i=%d h=%.3lf delta=%.3lf\n", period,bestInd,bestHeight,sqrt(bestDelta/n));

		/*
		for (int i = 0; i < n; ++i)
			r[i] = 0;
		for (int i = info.ind; i < n; i += info.p)
			r[i] = info.h;

		for (int i = hlen; i < n; ++i)
			for (int j = 0; j < hlen; ++j)
				r[i] += c[p][j] * r[i - j - 1];
		double scale = bestScale();
		info.h *= scale;
		//for (int i = 0; i < n; ++i) r[i] *= scale;
		for (int i = n-1; i >= hlen; --i) {
			int k = hlen - 1;
			while (k >= 0 && abs(c[p][k]) < 1e-8)
				--k;
			if (k < 0)
				continue;
			double S = t[i];
			for (int j = 0; j < k; ++j)
				S -= c[p][j] * r[i - j - 1];
			r[i - k - 1] = S / c[p][k];
		}
		*/
/*
if(__c1) {
FILE *fez = fopen("../visual/errz.tex", "w");
	//for(size_t i=0; i<info.towers.size(); ++i) t[info.towers[i]]=20000;
	for(int i=hlen; i<n; ++i)
		fprintf(fez, "%.5lf %.5lf\n", (double)(i+1), r[i]/32767.);
fclose(fez);
}
*/
		#undef s
	//printf("c=\n");
	//for(int j=0;j<hlen;++j)printf("%.3lf ",c[p][j]);printf("\n");
	}

}


void XSegment::get(double samples[]) {
if(__c1) {
	printf("get()\n");
	printf("ch=%d\n", channels);
	printf("n=%d\n", n);
	printf("hlen=%d\n", hlen);
}
	for (int p = 0; p < channels; ++p) {
		#define s(i) (samples[(i) * channels + (p)])
		for (int i = 0; i < hlen; ++i)
			s(i) = a[p][i];
		for (int i = hlen; i < n; ++i) {
			s(i) = 0;
			for (int j = 0; j < hlen; ++j)
				s(i) += s(i - j - 1) * c[p][j];
		}
		for (int i = 0; i < n; ++i)
			r[i] = 0.;
		for (int i = bestInd; i < n; i += period)
			r[i] = bestHeight;
		for (int i = hlen; i < n; ++i)
			for (int j = 0; j < hlen; ++j)
				r[i] += c[p][j] * r[i - j - 1];
if(__c1) continue;
		for (int i = 0; i < n; ++i) s(i) += r[i];
		#undef s
	}
}


XLpa::XLpa(QWidget *parent) : QWidget(parent) {
	progressBar = new QProgressBar();
	for (int i = 0; i < 5; ++i)
		label[i] = new QLabel();

	QGridLayout *layout = new QGridLayout();
	layout->addWidget(new QLabel("Sample Rate:"), 0, 0, 1, 1);
	layout->addWidget(label[0], 0, 1, 1, 1);
	layout->addWidget(new QLabel("Frames Rate:"), 1, 0, 1, 1);
	layout->addWidget(label[1], 1, 1, 1, 1);
	layout->addWidget(new QLabel("Window Width:"), 0, 3, 1, 1);
	layout->addWidget(label[2], 0, 4, 1, 1);
	layout->addWidget(new QLabel("Window Time:"), 1, 3, 1, 1);
	layout->addWidget(label[3], 1, 4, 1, 1);
	layout->addWidget(new QLabel("Window Head:"), 2, 3, 1, 1);
	layout->addWidget(label[4], 2, 4, 1, 1);
	layout->addWidget(new QLabel(), 3, 0, 1, 1);

	layout->addWidget(new QLabel("Progress:"), 4, 0, 1, 1);
	layout->addWidget(progressBar, 4, 1, 1, 4);
	setLayout(layout);
}


void XLpa::convert(QString outputFile) {
	out << "[XLpa::convert]" << endl;
	XPara para;
	std::ifstream input(inputFile.toStdString().c_str(), std::ios::binary);
	input.read((char*)(&para), sizeof(para));

	out << "[XLpa::convert]" << endl;
	out << "Input  : " << inputFile << endl;
	out << "Output : " << outputFile << endl;
	out << "Opened " << inputFile << "." << endl;
	out << "    Channels    : " << para.channels << endl;
	out << "    Sample rate : " << para.samplerate << endl;
	out << "    Frames      : " << para.frames << endl;
	out << "    Win Width   : " << para.window << endl;
	out << "    Win H Len   : " << para.hlen << endl;
	progressBar->setRange(0, 100);

	SndfileHandle output(outputFile.toStdString(), SFM_WRITE,
		SF_FORMAT_WAV | SF_FORMAT_PCM_16,
		para.channels, para.samplerate);

	double *buf = new double[para.window * para.channels];
	short *buf2 = new short[para.window * para.channels];
	XSegment *segment = new XSegment(para.channels, para.window, para.hlen);
	for (sf_count_t cur = 0; cur < para.frames; cur += para.window) {
		progressBar->setValue(progressBar->maximum() * cur / para.frames);

		int n = para.window;
		int hlen = para.hlen;
		if (cur + n > para.frames) {
			n = (int)(para.frames - cur);
			hlen = std::min(n, para.hlen);
			delete segment;
			segment = new XSegment(para.channels, n, hlen);
		}
		
		segment->load(input);
		segment->get(buf);
//if(cur==10000)for(int i=0; i<n; ++i)printf("ret[%d]=%d\n",i,buf[i]);

		for (int i = 0; i < para.window * para.channels; ++i)
			buf2[i] = (short)buf[i];
		sf_count_t writeCnt = output.write(buf2, n * para.channels);
		if (writeCnt != (sf_count_t)n * para.channels) {
			out << "ERROR [XLpa::convert]: writeCnt != n * ch" << endl;
			out << "   writeCnt = " << writeCnt << endl;
			out << "   n * ch   = " << n << endl;
			out << "   cur      = " << cur << endl;
		}
	}
	delete segment;
	delete buf;
	input.close();

	progressBar->setValue(100);
	sleep(100);
	progressBar->reset();
	out << "[XLpa::convert] Finished." << endl;
}


void XLpa::update(QString inputFile) {
	this->inputFile = inputFile;

	XPara para;
	std::ifstream file(inputFile.toStdString().c_str(), std::ios::binary);
	file.read((char*)(&para), sizeof(para));
	file.close();

	label[0]->setText(QString::number(para.samplerate));
	label[1]->setText(QString::number(para.frames));
	label[2]->setText(QString::number(para.window));
	double w = (double)para.window / (double)para.samplerate * 1000.;
	label[3]->setText(QString("%1 MS").arg(w, 0, 'f', 3));
	label[4]->setText(QString::number(para.hlen));
}
